
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2018 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f1xx_hal.h"
#include "cmsis_os.h"
#include "usb_device.h"

/* USER CODE BEGIN Includes */
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
//#include "motor_pid.h"
#include "mini_car2.h"
#include "motor_wheel.h"
//#include "usbd_cdc_if.h"
#include "wheel.h"
#include "i2c.eeprom.h"
#include "sabertoothdrive.h"
#include "rplidar.h"
#include "bluetooth.h"
#include "redcar_usb_command.h"

/* USER CODE END Includes */

/* Private variables ---------------------------------------------------------*/
I2C_HandleTypeDef hi2c1;

SPI_HandleTypeDef hspi1;
SPI_HandleTypeDef hspi2;

TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim7;

UART_HandleTypeDef huart5;
UART_HandleTypeDef huart1;
UART_HandleTypeDef huart2;
UART_HandleTypeDef huart3;
DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart1_tx;
DMA_HandleTypeDef hdma_usart2_rx;
DMA_HandleTypeDef hdma_usart2_tx;
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart3_tx;

osThreadId defaultTaskHandle;
osThreadId myTask02Handle;
osThreadId myTask03Handle;
osThreadId bluetoothTaskHandle;
osSemaphoreId carSourceBinarySemHandle;

/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_DMA_Init(void);
static void MX_I2C1_Init(void);
static void MX_SPI1_Init(void);
static void MX_SPI2_Init(void);
static void MX_TIM2_Init(void);
static void MX_TIM7_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_USART3_UART_Init(void);
static void MX_UART5_Init(void);
void StartDefaultTask(void const * argument);
void StartTask02(void const * argument);
void StartTask03(void const * argument);
void StartBluetoothTask(void const * argument);

void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
                                

/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/


#ifdef __GNUC__
/* With GCC/RAISONANCE, small printf (option LD Linker->Libraries->Small printf
   set to 'Yes') calls __io_putchar() */
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif /* __GNUC__ */

/**
  * @brief  Retargets the C library printf function to the USART.
  * @param  None
  * @retval None
  */
PUTCHAR_PROTOTYPE
{
    /* Place your implementation of fputc here */
    /* e.g. write a character to the USART */
    HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 10);
    return ch;
}
/* USER CODE END PFP */

/* USER CODE BEGIN 0 */
//#define       SET_COUNT   5
#define		    SYS_FREQ    72000000
#define 			MAX_SPEED   80
#define				PID_KP 0.5
#define FREQ 2879
#define motor_dutyCycle FREQ*0.60
#define  SPIN_KP 0.5
#define MAX_SPIN_SPEED 65
#define Joystick_Max_Speed 50
#define Joystick_Kp 0.6
#define LIDAR1NUM 2


//#define BUFFER_LENGTH 127


MINI_CAR_State foodcar_state;
MINI_CAR_State car_state;
MINI_CAR_State car_state_previous;
MINI_CAR_Mode current_car_mode,previous_car_mode;
T_Command cmd;
Bluetooth_Command_Action previous_action;
//Joystick_Command cmd;
FOOD_CAR food_car;


//unsigned char  Rx_data;
unsigned char  Rx_data3;
Lidar_Type lidar1;//lidar1 using USART2 PD5 PD6
//Lidar_Type lidar2;//lidar2 using USART3 PC10 PD11
//float atan_num;
unsigned char USB_Transfer_cplt=0;
char received_data[24];

uint16_t tick_usb=0;
extern T_USART_Buffer usart3;
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  *
  * @retval None
  */
int main(void)
{
  /* USER CODE BEGIN 1 */


    //init the pulse counter for 4 wheels


  /* USER CODE END 1 */

  /* MCU Configuration----------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_I2C1_Init();
  MX_SPI1_Init();
  MX_SPI2_Init();
  MX_TIM2_Init();
  MX_TIM7_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_USART3_UART_Init();
  MX_UART5_Init();
  /* USER CODE BEGIN 2 */
	MX_USB_DEVICE_Init();


//  motor_wheel_forward_setup();

    HAL_TIM_Base_Start_IT(&htim7);
		Start_Receive_lidardata(2);//start uasrt2
		HAL_UART_Receive_DMA(&huart3, &usart3.rx_data , 1);
	//	Start_Receive_lidardata(3);//Start usart3
  //  HAL_UART_Receive_DMA(&huart2, &Rx_data, 1);
	//	HAL_UART_Receive_DMA(&huart3, &Rx_data3, 1);
  //  printf ("SYSTEM STARTED...\r\n");

		foodcar_state=CAR_WHEEL_MOVE_FORWARD;
    car_state = CAR_WHEEL_MOVE_FORWARD;
    car_state_previous = car_state;
	
		HAL_GPIO_WritePin(Emegency_Stop_GPIO_Port,Emegency_Stop_Pin,GPIO_PIN_SET);
		autoBaud();

		// SET PWM FOR DRIVE LIDAR
//			 HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
//   __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, motor_dutyCycle);	
	 
//    Mini_Car_Init(SET_COUNT);
		Food_Car_Init(&food_car,PID_KP,MAX_SPEED,SPIN_KP,MAX_SPIN_SPEED);
		previous_car_mode=current_car_mode=CAR_FOLLOWER_MODE;
		previous_action=COMMAND_NOTHING;
		
		HAL_GPIO_WritePin(MCURUN_LITE_GPIO_Port,MCURUN_LITE_Pin,GPIO_PIN_SET);
	 
    UART5_Printf("Hello World \r\n");
		HAL_Delay(100);

		RPLIDAR_SCAN_START(LIDAR1NUM,&lidar1);
		
		

  /* USER CODE END 2 */

  /* USER CODE BEGIN RTOS_MUTEX */
    /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* Create the semaphores(s) */
  /* definition and creation of carSourceBinarySem */
  osSemaphoreDef(carSourceBinarySem);
  carSourceBinarySemHandle = osSemaphoreCreate(osSemaphore(carSourceBinarySem), 1);

  /* USER CODE BEGIN RTOS_SEMAPHORES */
    /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
    /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the thread(s) */
  /* definition and creation of defaultTask */
  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
  defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);

  /* definition and creation of myTask02 */
  osThreadDef(myTask02, StartTask02, osPriorityBelowNormal, 0, 128);
  myTask02Handle = osThreadCreate(osThread(myTask02), NULL);

  /* definition and creation of myTask03 */
  osThreadDef(myTask03, StartTask03, osPriorityNormal, 0, 128);
  myTask03Handle = osThreadCreate(osThread(myTask03), NULL);

  /* definition and creation of bluetoothTask */
  osThreadDef(bluetoothTask, StartBluetoothTask, osPriorityIdle, 0, 128);
  bluetoothTaskHandle = osThreadCreate(osThread(bluetoothTask), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
    /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_QUEUES */
    /* add queues, ... */

  /* USER CODE END RTOS_QUEUES */
 

  /* Start scheduler */
  osKernelStart();
  
  /* We should never get here as control is now taken by the scheduler */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
    while (1)
    {
  /* USER CODE END WHILE */

  /* USER CODE BEGIN 3 */
		}
  /* USER CODE END 3 */

}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{

  RCC_OscInitTypeDef RCC_OscInitStruct;
  RCC_ClkInitTypeDef RCC_ClkInitStruct;
  RCC_PeriphCLKInitTypeDef PeriphClkInit;

    /**Initializes the CPU, AHB and APB busses clocks 
    */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

    /**Initializes the CPU, AHB and APB busses clocks 
    */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB;
  PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_PLL_DIV1_5;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

    /**Configure the Systick interrupt time 
    */
  HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);

    /**Configure the Systick 
    */
  HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

  /* SysTick_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(SysTick_IRQn, 15, 0);
}

/* I2C1 init function */
static void MX_I2C1_Init(void)
{

  hi2c1.Instance = I2C1;
  hi2c1.Init.ClockSpeed = 100000;
  hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.OwnAddress2 = 0;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* SPI1 init function */
static void MX_SPI1_Init(void)
{

  /* SPI1 parameter configuration*/
  hspi1.Instance = SPI1;
  hspi1.Init.Mode = SPI_MODE_MASTER;
  hspi1.Init.Direction = SPI_DIRECTION_2LINES;
  hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
  hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;
  hspi1.Init.NSS = SPI_NSS_HARD_OUTPUT;
  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
  hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi1.Init.CRCPolynomial = 10;
  if (HAL_SPI_Init(&hspi1) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* SPI2 init function */
static void MX_SPI2_Init(void)
{

  /* SPI2 parameter configuration*/
  hspi2.Instance = SPI2;
  hspi2.Init.Mode = SPI_MODE_MASTER;
  hspi2.Init.Direction = SPI_DIRECTION_2LINES;
  hspi2.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi2.Init.CLKPolarity = SPI_POLARITY_LOW;
  hspi2.Init.CLKPhase = SPI_PHASE_1EDGE;
  hspi2.Init.NSS = SPI_NSS_HARD_OUTPUT;
  hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
  hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi2.Init.CRCPolynomial = 10;
  if (HAL_SPI_Init(&hspi2) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* TIM2 init function */
static void MX_TIM2_Init(void)
{

  TIM_MasterConfigTypeDef sMasterConfig;
  TIM_OC_InitTypeDef sConfigOC;

  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 0;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 2879;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  HAL_TIM_MspPostInit(&htim2);

}

/* TIM7 init function */
static void MX_TIM7_Init(void)
{

  TIM_MasterConfigTypeDef sMasterConfig;

  htim7.Instance = TIM7;
  htim7.Init.Prescaler = 11;
  htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim7.Init.Period = 59999;
  htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* UART5 init function */
static void MX_UART5_Init(void)
{

  huart5.Instance = UART5;
  huart5.Init.BaudRate = 115200;
  huart5.Init.WordLength = UART_WORDLENGTH_8B;
  huart5.Init.StopBits = UART_STOPBITS_1;
  huart5.Init.Parity = UART_PARITY_NONE;
  huart5.Init.Mode = UART_MODE_TX_RX;
  huart5.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart5.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart5) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* USART1 init function */
static void MX_USART1_UART_Init(void)
{

  huart1.Instance = USART1;
  huart1.Init.BaudRate = 9600;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
  huart1.Init.Mode = UART_MODE_TX_RX;
  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart1) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* USART2 init function */
static void MX_USART2_UART_Init(void)
{

  huart2.Instance = USART2;
  huart2.Init.BaudRate = 115200;
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart2) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* USART3 init function */
static void MX_USART3_UART_Init(void)
{

  huart3.Instance = USART3;
  huart3.Init.BaudRate = 9600;
  huart3.Init.WordLength = UART_WORDLENGTH_8B;
  huart3.Init.StopBits = UART_STOPBITS_1;
  huart3.Init.Parity = UART_PARITY_NONE;
  huart3.Init.Mode = UART_MODE_TX_RX;
  huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart3.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart3) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/** 
  * Enable DMA controller clock
  */
static void MX_DMA_Init(void) 
{
  /* DMA controller clock enable */
  __HAL_RCC_DMA1_CLK_ENABLE();

  /* DMA interrupt init */
  /* DMA1_Channel2_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
  /* DMA1_Channel3_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn);
  /* DMA1_Channel4_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn);
  /* DMA1_Channel5_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
  /* DMA1_Channel6_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel6_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel6_IRQn);
  /* DMA1_Channel7_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel7_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel7_IRQn);

}

/** Configure pins as 
        * Analog 
        * Input 
        * Output
        * EVENT_OUT
        * EXTI
*/
static void MX_GPIO_Init(void)
{

  GPIO_InitTypeDef GPIO_InitStruct;

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOE_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
  __HAL_RCC_GPIOD_CLK_ENABLE();
  __HAL_RCC_GPIOG_CLK_ENABLE();
  __HAL_RCC_GPIOC_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(HINDRANCE_LITE_GPIO_Port, HINDRANCE_LITE_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(MCURUN_LITE_GPIO_Port, MCURUN_LITE_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(Emegency_Stop_GPIO_Port, Emegency_Stop_Pin, GPIO_PIN_SET);

  /*Configure GPIO pin : HINDRANCE_LITE_Pin */
  GPIO_InitStruct.Pin = HINDRANCE_LITE_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(HINDRANCE_LITE_GPIO_Port, &GPIO_InitStruct);

  /*Configure GPIO pin : MCURUN_LITE_Pin */
  GPIO_InitStruct.Pin = MCURUN_LITE_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(MCURUN_LITE_GPIO_Port, &GPIO_InitStruct);

  /*Configure GPIO pins : IR_1_Pin IR_2_Pin IR_3_Pin IR_4_Pin 
                           IR_6_Pin IR_7_Pin */
  GPIO_InitStruct.Pin = IR_1_Pin|IR_2_Pin|IR_3_Pin|IR_4_Pin 
                          |IR_6_Pin|IR_7_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);

  /*Configure GPIO pin : Emegency_Stop_Pin */
  GPIO_InitStruct.Pin = Emegency_Stop_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(Emegency_Stop_GPIO_Port, &GPIO_InitStruct);

  /*Configure GPIO pin : BUMPER_Pin */
  GPIO_InitStruct.Pin = BUMPER_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(BUMPER_GPIO_Port, &GPIO_InitStruct);

  /*Configure GPIO pins : IR_8_Pin IR_9_Pin PG4 IR_10_Pin */
  GPIO_InitStruct.Pin = IR_8_Pin|IR_9_Pin|GPIO_PIN_4|IR_10_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
  HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);

  /*Configure GPIO pin : PE0 */
  GPIO_InitStruct.Pin = GPIO_PIN_0;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
  HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);

}

/* USER CODE BEGIN 4 */




static char uart5_tx_buffer[128];
uint8_t BUFFER_LENGTH5=127;

void UART5_Printf(const char* format,...)
{
    int i=-1;

    //uint8_t result;


    va_list arg;
    va_start(arg,format);
    i=vsnprintf((char *)&uart5_tx_buffer,BUFFER_LENGTH5,format,arg);
	//  i=vsprintf((char *)&uart2_tx_buffer,format,arg);


    if(i>0)
    {
			uint8_t a=0;
			while((a<BUFFER_LENGTH5)&&uart5_tx_buffer[a])
			{
				 HAL_UART_Transmit(&huart5, (uint8_t *)&uart5_tx_buffer[a++], 1,10);
				// a++;

			}
			// HAL_UART_Transmit_DMA(&huart3, (uint8_t *)&uart3_tx_buffer, i);

    }
    va_end(arg);
}
/* USER CODE END 4 */

/* StartDefaultTask function */
void StartDefaultTask(void const * argument)
{
  /* init code for USB_DEVICE */
  //MX_USB_DEVICE_Init();

  /* USER CODE BEGIN 5 */

    /*Infinit Loop */
    for(;;)
    {
		//   UART5_Printf("Hello Task01 \r\n");
        //check the difference between last count and current count

				if((foodcar_state==CAR_STOPPED)||(HAL_GPIO_ReadPin( BUMPER_GPIO_Port,BUMPER_Pin)==GPIO_PIN_RESET))
				{
					//Emergency Stop
					Food_Car_EmergencyStop(&food_car);				
					HAL_GPIO_WritePin(HINDRANCE_LITE_GPIO_Port,HINDRANCE_LITE_Pin,GPIO_PIN_SET);
					previous_action=CMD_BLUETOOTH_CLOSE;
					osDelay(2000);				
				
				}
				else 
				{

					
						HAL_GPIO_WritePin(HINDRANCE_LITE_GPIO_Port,HINDRANCE_LITE_Pin,GPIO_PIN_RESET);
						HAL_GPIO_WritePin(MCURUN_LITE_GPIO_Port,MCURUN_LITE_Pin,GPIO_PIN_SET);							

						if(current_car_mode!=previous_car_mode)
						{
							Mini_Car_Stop(&food_car);
							previous_car_mode=current_car_mode;
						if(current_car_mode==CAR_BLUETOOTH_MODE)
							Car_Joystick_Mode_Init(&food_car, Joystick_Kp, Joystick_Max_Speed);
						else if(current_car_mode==CAR_FOLLOWER_MODE)
							Food_Car_Init(&food_car,PID_KP,MAX_SPEED,SPIN_KP,MAX_SPIN_SPEED);

						}


						if(current_car_mode==CAR_FOLLOWER_MODE)
						{
//							int val;
//							val = osSemaphoreWait (carSourceBinarySemHandle, 3);
//							if(val>0)
//							{
								xyz2speed2(&food_car);	
								Mini_Car_Run(&food_car);	
								osSemaphoreRelease (carSourceBinarySemHandle); 
//							}
						
						}
//						else
//						{
//						 Car_Joystick_Control_Mode(&food_car, cmd);
//						
//						}
//						Mini_Car_Run(&food_car);	
						else if(cmd.action!=previous_action)
					 {
							Mini_Car_Stop(&food_car);
							Food_Car_BluetoothControl(&food_car,cmd);
							previous_action=cmd.action;

					 }			

				 
				//my_printf("This is Task 1 Get the Semaphore\r\n");
			  
			
			}

			osDelay(10);

    }
	
  /* USER CODE END 5 */ 
}

/* StartTask02 function */
void StartTask02(void const * argument)
{
  /* USER CODE BEGIN StartTask02 */
	 //   UART5_Printf("Hello Task02 \r\n");
    /* Infinite loop */
    for(;;)
    {         
	   		//UART5_Printf("Hello Task02 \r\n");                               
				
        //else if(USB_Transfer_cplt==1)
			//osSemaphoreWait (carSourceBinarySemHandle, osWaitForever);
			  if(USB_Transfer_cplt==1)
        {
					USB_CMD cmd;
					cmd=Get_USB_data((char *)received_data);
					if(cmd.error_code==0)
					{
						food_car.target_coordinate_x=cmd.xf;
						food_car.taget_coordinate_z=cmd.zf;
						food_car.target_coord_atanNum=cmd.atan_num;
						//my_printf("Receive from USB:%f,%f,%f,%f\r\n",cmd.xf,cmd.yf,cmd.zf,cmd.atan_num);
					
					}
					else
					{
						//my_printf("Receive Error\r\n");
					
					}
					
          USB_Transfer_cplt = 0;


          memset(received_data,0,sizeof(received_data));
					tick_usb=0;

					}
				//my_printf("This is Task 2 Get the Semaphore\r\n");
	//			osSemaphoreRelease (carSourceBinarySemHandle); 
					tick_usb++;
					if(tick_usb>80)
					{
						food_car.target_coordinate_x=0;
						food_car.taget_coordinate_z=0;
						food_car.target_coord_atanNum=0;
					
					}
				
          osDelay(10);
	//					UART5_Printf("Task02 end\r\n");
				

        
    }
  /* USER CODE END StartTask02 */
}

/* StartTask03 function */
void StartTask03(void const * argument)
{
  /* USER CODE BEGIN StartTask03 */
//	static uint8_t collect_num=0;

  /* Infinite loop */
  for(;;)
  {
		 //UART5_Printf("Hello Task03 \r\n");
		if(isReceiveOvertime(LIDAR1NUM)==1)
		{
			//UART5_Printf("Receive over time\r\n");
		//	RPLIDAR_RESTARTSCAN(LIDAR1NUM,&lidar1);
			HAL_NVIC_SystemReset();
		//	collect_num=0;
		
		}
		else if(collect_LIDARdata(LIDAR1NUM,&lidar1)==1)
		{
			uint8_t stop_state=0;
			stop_state=Get_CarState_Lidar(&lidar1,food_car.run_speed, 320,-320);

			if(stop_state==0)
				foodcar_state=CAR_WHEEL_MOVE_FORWARD;
			else
				foodcar_state=CAR_STOPPED;
		

	} 
	


    osDelay(10);
  }
  /* USER CODE END StartTask03 */
}

/* StartBluetoothTask function */
void StartBluetoothTask(void const * argument)
{
  /* USER CODE BEGIN StartBluetoothTask */
  /* Infinite loop */
  for(;;)
  {
		if(usart3.rx_complete==1)
		{
			cmd=lscm_parse_received_command_usart3();
//						 
			if(cmd.action==CMD_BLUETOOTH_OPEN)
			{
				current_car_mode=CAR_BLUETOOTH_MODE;
			}
			else if(cmd.action==CMD_BLUETOOTH_CLOSE)
			{
				current_car_mode=CAR_FOLLOWER_MODE;
			
			}
//			my_printf("%s\r\n",usart3.rx_buffer);

			
//			cmd=lscm_parse_received_command_usart3_joystick();
//			if(cmd.action!=CMD_ERROR)
//			{
//				if(cmd.action==CMD_BLUETOOTH_OPEN)
//				{
//					current_car_mode=CAR_BLUETOOTH_MODE;
//					//Car_Joystick_Mode_Init(&food_car, Joystick_Kp, Joystick_Max_Speed);
//					//my_printf("%d\r\n",cmd.action);
//				}
//				else if(cmd.action==CMD_BLUETOOTH_CLOSE)
//				{
//					current_car_mode=CAR_FOLLOWER_MODE;
//					//Food_Car_Init(&food_car,PID_KP,MAX_SPEED,SPIN_KP,MAX_SPIN_SPEED);
//				
//				}
//			
//			}
//			else
//			 my_printf("error\r\n");

			
			usart3.rx_complete=0;
			usart3_reset_buffer();
		
		}
    osDelay(100);
  }
  /* USER CODE END StartBluetoothTask */
}

/**
  * @brief  This function is executed in case of error occurrence.
  * @param  file: The file name as string.
  * @param  line: The line in file as a number.
  * @retval None
  */
void _Error_Handler(char *file, int line)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  while(1) 
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t* file, uint32_t line)
{ 
  /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
      ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/**
  * @}
  */

/**
  * @}
  */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
